import os
import yaml

from ament_index_python.packages import get_package_share_directory
from launch.substitutions import Command
from launch_ros.actions import Node

launch_params = yaml.safe_load(open(os.path.join(
    get_package_share_directory('rm_vision_bringup'), 'config', 'launch_params.yaml')))
robot_type_param = yaml.safe_load(open(os.path.join(
    get_package_share_directory('rm_vision_bringup'), 'config', 'robot_type.yaml')))

this_robot_tfs = launch_params[robot_type_param["robot_type"]]

robot_description = Command(['xacro ', os.path.join(
    get_package_share_directory('rm_gimbal_description'), 'urdf', 'rm_gimbal.urdf.xacro'),
    ' imu2gimbal_link_xyz:=', this_robot_tfs['imu2gimbal_link']['xyz'], ' imu2gimbal_link_rpy:=', this_robot_tfs['imu2gimbal_link']['rpy'],
    ' camera2gimbal_link_xyz:=', this_robot_tfs['camera2gimbal_link']['xyz'], ' camera2gimbal_link_rpy:=', this_robot_tfs['camera2gimbal_link']['rpy'],
    ' gun2gimbal_link_xyz:=', this_robot_tfs['gun2gimbal_link']['xyz'], ' gun2gimbal_link_rpy:=', this_robot_tfs['gun2gimbal_link']['rpy'],
    ])

# 发布static_transform gimbal_link 到 camera_color_optical_frame

# this_robot_tfs['camera2gimbal_link']['xyz'], ' camera2gimbal_link_rpy:=', this_robot_tfs['camera2gimbal_link']['rpy']
# 删除双引号，用空格分隔
# rpy = this_robot_tfs['camera2gimbal_link']['rpy'].replace('"', '').split(' ')
# xyz = this_robot_tfs['camera2gimbal_link']['xyz'].replace('"', '').split(' ')


# gimbal_link2camera_color_optical_frame = Node(
#     package='tf2_ros',
#     executable='static_transform_publisher',
#     arguments=[xyz[0],
#                xyz[1],
#                xyz[2],
#                rpy[0],
#                rpy[1],
#                rpy[2],
#                'gimbal_link', 'camera_color_optical_frame'],
# )

robot_state_publisher = Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    parameters=[{'robot_description': robot_description,
                 'publish_frequency': 1000.0}]
)

node_params = os.path.join(
    get_package_share_directory('rm_vision_bringup'), 'config', 'node_params.yaml')

tracker_node = Node(
    package='armor_tracker',
    executable='armor_tracker_node',
    output='both',
    emulate_tty=True,
    parameters=[node_params],
    # ros_arguments=['--log-level', 'armor_tracker:='+launch_params['tracker_log_level']],
)
